#include "ros/ros.h"
#include "turtlesim/Spawn.h"
#include "geometry_msgs/Twist.h"
#define pi 3.14159265358979323846

int main(int argc, char** argv) {
    //初始化
    ros::init(argc, argv, "write_sudolab");
    ros::NodeHandle nh;
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    ros::Publisher pub1 = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 10);
    geometry_msgs::Twist twist1;
    //创建第二个乌龟画s
    turtlesim::Spawn spawn2;
    spawn2.request.name = "turtle2";
    spawn2.request.theta = -pi / 10;
    spawn2.request.x = 1.3861125 - 0.7;
    spawn2.request.y = 5.544445;
    client.waitForExistence();
    bool flag2 = client.call(spawn2);
    ros::Publisher pub2 = nh.advertise <geometry_msgs::Twist>("turtle2/cmd_vel", 10);
    geometry_msgs::Twist twist2;
    //创建第三个乌龟画u
    turtlesim::Spawn spawn3;
    spawn3.request.name = "turtle3";
    spawn3.request.theta = pi / 2;
    spawn3.request.x = 2 * 1.3861125 - 0.6;
    spawn3.request.y = 5.544445 + 0.2;
    client.waitForExistence();
    bool flag3 = client.call(spawn3);
    ros::Publisher pub3 = nh.advertise<geometry_msgs::Twist>("turtle3/cmd_vel", 10);
    geometry_msgs::Twist twist3;
    //创建第四个乌龟画d
    turtlesim::Spawn spawn4;
    spawn4.request.name = "turtle4";
    spawn4.request.theta = pi;
    spawn4.request.x = 3 * 1.3861125;
    spawn4.request.y = 5.544445;
    client.waitForExistence();
    bool flag4 = client.call(spawn4);
    ros::Publisher pub4 = nh.advertise<geometry_msgs::Twist>("turtle4/cmd_vel", 10);
    geometry_msgs::Twist twist4;
    //第一个乌龟画o
    //创建第五个乌龟画l
    turtlesim::Spawn spawn5;
    spawn5.request.name = "turtle5";
    spawn5.request.theta = pi / 2;
    spawn5.request.x = 5 * 1.3861125;
    spawn5.request.y = 5.544445;
    client.waitForExistence();
    bool flag5 = client.call(spawn5);
    ros::Publisher pub5 = nh.advertise<geometry_msgs::Twist>("turtle5/cmd_vel", 10);
    geometry_msgs::Twist twist5;
    //创建第六个乌龟画a
    turtlesim::Spawn spawn6;
    spawn6.request.name = "turtle6";
    spawn6.request.theta = pi / 2;
    spawn6.request.x = 6 * 1.3861125 + 0.5;
    spawn6.request.y = 5.544445;
    client.waitForExistence();
    bool flag6 = client.call(spawn6);
    ros::Publisher pub6 = nh.advertise<geometry_msgs::Twist>("turtle6/cmd_vel", 10);
    geometry_msgs::Twist twist6;
    //创建第七个乌龟画b
    turtlesim::Spawn spawn7;
    spawn7.request.name = "turtle7";
    spawn7.request.theta = pi / 2;
    spawn7.request.x = 7 * 1.3861125;
    spawn7.request.y = 5.544445;
    client.waitForExistence();
    bool flag7 = client.call(spawn7);
    ros::Publisher pub7 = nh.advertise<geometry_msgs::Twist>("turtle7/cmd_vel", 10);
    geometry_msgs::Twist twist7;
    //控制乌龟运动
    twist1.linear.x = 0;
    twist1.angular.z = 0;
    pub1.publish(twist1);
    ros::Duration(0.5).sleep();

    twist2.linear.x = 1;
    twist2.angular.z = pi;
    twist3.linear.x = 1;
    twist3.angular.z = 0;
    twist4.linear.x = 2;
    twist4.angular.z = -pi;
    twist1.linear.x = 0.7;
    twist1.angular.z = 2 * pi / 5;
    pub1.publish(twist1);
    twist5.linear.x = 2;
    twist5.angular.z = 0;
    twist6.linear.x = 1;
    twist6.angular.z = 0;
    twist7.linear.x = 2;
    twist7.angular.z = 0;
    pub2.publish(twist2);
    pub3.publish(twist3);
    pub4.publish(twist4);
    pub1.publish(twist1);
    pub5.publish(twist5);
    pub6.publish(twist6);
    pub7.publish(twist7);
    ros::Duration(1).sleep();

    twist2.linear.x = 0.3;
    twist2.angular.z = -pi / 4;
    twist3.linear.x = 0;
    twist3.angular.z = pi + 0.01;
    twist4.linear.x = 0.0;
    twist4.angular.z = pi / 2;
    twist5.linear.x = 0;
    twist5.angular.z = pi;
    twist6.linear.x = 0;
    twist6.angular.z = pi / 2;
    twist7.linear.x = 0;
    twist7.angular.z = pi;
    pub2.publish(twist2);
    pub3.publish(twist3);
    pub4.publish(twist4);
    pub1.publish(twist1);
    pub5.publish(twist5);
    pub6.publish(twist6);
    pub7.publish(twist7);
    ros::Duration(1).sleep();

    twist3.linear.x = 1;
    twist3.angular.z = 0;
    twist4.linear.x = 1;
    twist4.angular.z = 0;
    twist5.linear.x = 2;
    twist5.angular.z = 0;
    twist6.linear.x = 1;
    twist6.angular.z = pi / 2;
    twist7.linear.x = 2;
    twist7.angular.z = 0;
    pub2.publish(twist2);
    pub3.publish(twist3);
    pub4.publish(twist4);
    pub1.publish(twist1);
    pub5.publish(twist5);
    pub6.publish(twist6);
    pub7.publish(twist7);
    ros::Duration(1).sleep();

    twist3.linear.x = 1;
    twist3.angular.z = pi;
    twist4.linear.x = 0.0;
    twist4.angular.z = pi + 0.03;
    twist6.linear.x = 1;
    twist6.angular.z = pi;
    twist7.linear.x = 0;
    twist7.angular.z = pi / 2;
    pub2.publish(twist2);
    pub3.publish(twist3);
    pub4.publish(twist4);
    pub1.publish(twist1);
    pub6.publish(twist6);
    pub7.publish(twist7);
    ros::Duration(1).sleep();

    twist3.linear.x = 1;
    twist3.angular.z = 0;
    twist4.linear.x = 2.1;
    twist4.angular.z = 0;
    twist7.linear.x = 2;
    twist7.angular.z = pi;
    twist6.linear.x = 0;
    twist6.angular.z = pi;
    pub2.publish(twist2);
    pub3.publish(twist3);
    pub4.publish(twist4);
    pub1.publish(twist1);
    pub6.publish(twist6);
    pub7.publish(twist7);
    ros::Duration(1).sleep();

    twist3.linear.x = 0;
    twist3.angular.z = pi;
    twist4.linear.x = 0.4;
    twist4.angular.z = pi * 2 / 3;
    twist5.linear.x = 0.4;
    twist5.angular.z = pi * 2 / 3;
    twist6.linear.x = 0.4;
    twist6.angular.z = pi * 2 / 3;
    pub3.publish(twist3);
    pub4.publish(twist4);
    pub5.publish(twist5);
    pub6.publish(twist6);
    ros::Duration(1).sleep();

    twist3.linear.x = 1.1;
    twist3.angular.z = 0;
    pub3.publish(twist3);
    ros::Duration(0.2).sleep();

    return 0;
}